#include "RSIM_INC.h"
#include "RigidBodyNode.h"
#include "ABAData.h"
#include "EOMData.h"

#include <iomanip>

using namespace RSIM;

int main(){

        double mass = 1.0;
        Mat3x3 I;
        I.Diagonal(1.0,2.0,3.0);
        Rotation R1;
        Rotation R2(1,2,3,"Body123");	
        I = R2*I*(~R2);	

        MultibodySystem MBS;
        MBS.addSystemAcceleration(Vect3(0.0,-9.81,0.0));
        RigidBodyNode *tmp1, *tmp2,*tmp3;        
        
        const int& nbodies = 16;	
        const double& dim = 0.5;

        // Root body connected to ground via Pin joint. For a pin joint, the Z axis of frames M and F (see Bods.pdf) are always coincident.	
        // This is node no 0, body number 1 (0^th body is supposed to be the ground body)
        tmp1 = MBS.setRoot(RigidBody("Pin",MassProperties(mass, I),
                                Transform(Vect3(-dim,0.0,0.0)), // T_BF
                                Transform(Vect3(0,0,0))) // T_PF
                                );
                                
        for(int i=1;i<nbodies;++i){
                tmp2 = MBS.addNodeTo(tmp1, // Parent node
                                     RigidBody("Pin",MassProperties(mass, I),
                                               Transform(Vect3(-dim,0.0,0.0)), // T_BF
                                               Transform(R2,Vect3(dim,0.0,0.0))) // T_PF
                                               );      
                tmp1 = tmp2;
        }        
                                
        MBS.setTopologyMaps();
        State state(MBS);	
        MBS.initEOM();
        MBS.initABA();
        
        
        // Set the quaternion associated with the ball joint
        // (Node 1, Body 2)
        Quaternion Q(R2);
        double u[]={1,2,3};
        for(int i=1;i<=nbodies;++i){
                MBS.updQ(BodyIndex(i),u,state);
                MBS.updU(BodyIndex(i),u,state);
        }        
        
        // This function should update all the joint transforms from the updated state.
        MBS.updSystemPosition(state);
        MBS.updSystemVelocity(state);
        MBS.updSystemAcceleration(state);
        
//         const RigidBody& bod = MBS.getBody(BodyIndex(16));
//         cout<<MBS.getBody(BodyIndex(1))->data_->getRotationNF()<<endl;
        
        // cout<<MBS.getBody(BodyIndex(1))->data_->getRotationNF()*MBS.getBody(BodyIndex(1))->data_->getAngularVelocity()<<endl;
        
        state.printUDot();
        cin.get();
        Mat6x6 TmP;
        Vect6 f;
        FastOps::spatial_basis_shift_mat( MBS.getBody(BodyIndex(1))->data_->getRotationNF().Ptr(),
                                                MBS.getBody(BodyIndex(1))->data_->getABAData()->getIk3()->Ptr(),
                                                TmP.wPtr());
        FastOps::spatial_basis_shift_vec( MBS.getBody(BodyIndex(1))->data_->getRotationNF().Ptr(),
                                                MBS.getBody(BodyIndex(1))->data_->getABAData()->getFk3()->Ptr(),
                                                f.wPtr());
        cout<<TmP<<endl<<endl;
        cout<<f<<endl<<endl;
//         double d[6];
//         for(int i=1;i<=4;++i){
//         FastOps::spatial_basis_shift_vec(MBS.getBody(BodyIndex(i))->data_->getRotationNF().Ptr(),       
//                                          MBS.getBody(BodyIndex(i))->data_->getAkt().Ptr(),
//                                          d);
//         cout<<Vect6(d)<<endl<<endl;
//         }
//         FastOps::spatial_basis_shift_vec(MBS.getBody(BodyIndex(1))->data_->getRotationNF().Ptr(),       
//                                          MBS.getBody(BodyIndex(1))->data_->getEOMData()->getdFk1()->Ptr(),
//                                          d);
//         cout<<Vect6(d)<<endl<<endl;     
//         FastOps::spatial_basis_shift_vec(MBS.getBody(BodyIndex(1))->data_->getRotationNF().Ptr(),       
//                                           MBS.getBody(BodyIndex(1))->data_->getABAData()->getFk3()->Ptr(),
//                                           d);
//      */
//         cout<<Vect6(d)<<endl<<endl;     
        return 0;
}
